#pragma once
#ifndef SAMPLER_H
#define SAMPLER_H

#include "basic_typedef.h"
#include "CollisionDetector.h"
#include <CGAL/Cartesian.h>
#include <iostream>

class SingleRobotSampler
{
public:
	
	////////////////////////
	// CTORS
	////////////////////////
	/*	Parameters:
			radius	- radius of robot
			Room	- describes the boundary of the scenario
			CollisionDetector	- Collision detection black box
			*/

	SingleRobotSampler(Polygon_2 robot_polygon, Room room, double seed = -1)
	{	
		Point_2 tl = room.first;
		Point_2 br = room.second;

		double height_of_robot = CGAL::to_double(robot_polygon.top_vertex()[0].y()) - CGAL::to_double(robot_polygon.bottom_vertex()[0].y());
		double width_of_robot = CGAL::to_double(robot_polygon.right_vertex()[0].x()) - CGAL::to_double(robot_polygon.left_vertex()[0].x());

		xmin = CGAL::to_double(tl.x()) + width_of_robot;
		xmax = CGAL::to_double(br.x()) - width_of_robot;
		ymax = CGAL::to_double(br.y()) - height_of_robot;
		ymin = CGAL::to_double(tl.y()) + height_of_robot;
		lenx = xmax - xmin;
		leny = ymax - ymin;

		if (seed == -1)
			seed = time(NULL);
		srand ( seed );
	}

	  ~SingleRobotSampler(void){};

	////////////////////////
	// Queries
	////////////////////////

	/*	Samples a point in the configuration space of the robot
		defined by the bounding box of the scenario.
		This point can be valid or invalid.	*/
	Point_2 generate_sample()
	{
		double randx = xmin + lenx * ((double)rand())/((double)RAND_MAX);
		double randy = ymin + leny * ((double)rand())/((double)RAND_MAX);

		Point_2 p(randx,randy);

		return p;
	}

	// room variables
	double xmin;
	double xmax;
	double ymin;
	double ymax;
	double lenx;
	double leny;
};

class MultiRobotsSampler
{
public:

	MultiRobotsSampler(vector<Polygon_2> robot_polygons_vector, Room room, CollisionDetector *col, double seed = -1) : 
	  m_robot_polygons_vector(robot_polygons_vector), m_col(col)
	  {	
		  for (vector<Polygon_2>::iterator robots_it = robot_polygons_vector.begin();
			  robots_it != robot_polygons_vector.end();
			  ++robots_it)
		  {
			  SingleRobotSampler* robot_sampler_ptr = new SingleRobotSampler(*robots_it,room,seed);
			  m_robot_samplers.push_back(robot_sampler_ptr);
		  }
	  }

	  ~MultiRobotsSampler(void){};

	  ConfSet generate_sample_no_obstacles()
	  {
		  while(true)
		  {
			  ConfSet new_config = generate_sample();

			  if (m_col->valid_conf(new_config[0],new_config[1]))
			  {
				  return new_config;
			  }
		  }
	  }

private:

	ConfSet generate_sample()
	{
		ConfSet robots_config;

		for (vector<SingleRobotSampler*>::iterator samplers_it = m_robot_samplers.begin();
			 samplers_it != m_robot_samplers.end();
			 ++samplers_it)
		{
			robots_config.push_back((*samplers_it)->generate_sample());
		}

		return robots_config;
	}

	////////////////////////
	// Data Members
	////////////////////////
	vector<Polygon_2>				m_robot_polygons_vector;
	CollisionDetector*				m_col;	// Collision detector
	vector<SingleRobotSampler*>		m_robot_samplers;
};

#endif